#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S3,     IR_R,           sensorI2CCustom)
#pragma config(Sensor, S4,     IR_L,           sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     B_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     F_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     B_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     F_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Pitch,         tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     motorK,        tmotorTetrix, PIDControl, encoder)
#pragma config(Servo,  srvo_S2_C3_1,    Hook,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C3_2,    Scooper,              tServoStandard)
#pragma config(Servo,  srvo_S2_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "hitechnic-irseeker-v2.h"
#include "AutonomousBase.c"
#pragma DebuggerWindows("debugStream")

task main()
{
int R_dir, L_dir, IR_Off, Sector;
	bool track, range = false;

	const	int IR_AR[9][9] = { 	//9X9 array for sector value. 0 = DNE, negative = behind, 1, 73, 64, 10 is centered, others labled left to right
		{-1, -3, 0, 0, 0, 0, 0, 0, 0},		/*1:1, 2:1, 3:1, 4:1, 5:1, 6:1, 7:1, 8:1, 9:1*/
		{0, 2, 3, 4, 11, 12, 13, 1, 0}, /*1:2, 2:2, 3:2, 4:2, 5:2, 6:2, 7:2, 8:2, 9:2*/
		{0, 0, 5, 6, 9, 14, 73, 16, 0}, /*1:3, 2:3, 3:3, 4:3, 5:3, 6:3, 7:3, 8:3, 9:3*/
		{0, 0, 0, 7, 8, 64, 15, 17, 0}, /*1:4, 2:4, 3:4, 4:4, 5:4, 6:4, 7:4, 8:4, 8:4*/
		{0, 0, 0, 0, 10, 21, 20, 18, 0},/*1:5, 2:5, 3:5, 4:5, 5:5, 6:5, 7:5, 8:5, 9:5*/
		{0, 0, 0, 0, 0, 22, 23, 19, 0 },/*1:6, 2:6, 3:6, 4:6, 5:6, 6:6, 7:6, 8:6, 9:6*/
		{0, 0, 0, 0, 0, 0, 24, 25, 0} , /*1:7, 2:7, 3:7, 4:7, 5:7, 6:7, 7:7, 8:7 9:7*/
		{0, 0, 0, 0, 0, 0, 0, 26, 0}  , /*1:8, 2:8, 3:8, 4:8, 5:8, 6:8, 7:8, 8:8, 9:8*/
		{0, 0, 0, 0, 0, 0, 0, -4, -2}};  /*1:9, 2:9, 3:9, 4:9, 5:9, 6:9, 7:9, 8:9, 9:9*/


	while(true)
	{
		R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
		IR_Off = abs(L_dir - 5);
		if(R_dir > 0 && L_dir > 0 )
			Sector = IR_AR[R_dir][L_dir];					//Grab which sector the beacon is in
		writeDebugStreamLine("L_dir = %d R_dir = %d", L_dir, R_dir);
		wait10Msec(25);

			if(R_dir == -1 || L_dir == -1)
			{
				motor[F_Left] = 0;
	   		motor[B_Left] = 0;
		  	motor[F_Right] = 0;
		  	motor[B_Right] = 0;
	    }

		if(Sector == 7 || Sector == 10)	//Foward Curve Left
		{
			 R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		   L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   //	if(R_dir > 0 && L_dir > 0 )
	   //		Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = -80;
			motor[B_Left] = -80;
			motor[F_Right] = -50;
			motor[B_Right] = -50;
		}

		if(Sector == 9 || Sector == 13)	//Rotate Left
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	  // 	if(R_dir > 0 && L_dir > 0 )
	  // 		Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = 40;
			motor[B_Left] = 40;
			motor[F_Right] = 40;
			motor[B_Right] = 40;
		}

		if(Sector == 14)	//Slow foward Left
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   	//if(R_dir > 0 && L_dir > 0 )
	   	//	Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = -40;
			motor[B_Left] = -40;
			motor[F_Right] = -10;
			motor[B_Right] = -10;
		}

		if(Sector == 64)	//Slow Foward
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   	//if(R_dir > 0 && L_dir > 0 )
	   	//	Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = -30;
			motor[B_Left] = -30;
			motor[F_Right] = 30;
			motor[B_Right] = 30;
		}

		if(Sector == 21 || Sector == 22)	//Foward Right
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   //	if(R_dir > 0 && L_dir > 0 )
	   //		Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = 50;
			motor[B_Left] = 50;
			motor[F_Right] = 80;
			motor[B_Right] = 80;
		}

		if(Sector == 20 || Sector == 16)	//Rotate Right
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   	//if(R_dir > 0 && L_dir > 0 )
	   	//	Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = -40;
			motor[B_Left] = -40;
			motor[F_Right] = -40;
			motor[B_Right] = -40;
	  }

		if(Sector == 15)	//Slow Foward Right
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   	//if(R_dir > 0 && L_dir > 0 )
	   		//Sector = IR_AR[R_dir][L_dir];
			motor[F_Left] = 10;
			motor[B_Left] = 10;
			motor[F_Right] = 40;
			motor[B_Right] = 40;
		}

		if(Sector == 5)	//Foward
		{
			R_dir = HTIRS2readACDir(IR_R) - 1;		//Subtract 1 to fit in array
		  L_dir = HTIRS2readACDir(IR_L) - 1;		//Subtract 1 to fit in array
	   	//if(R_dir > 0 && L_dir > 0 )
	   	//	Sector = IR_AR[R_dir][L_dir];
	    motor[F_Left] = 80;
			motor[B_Left] = 80;
			motor[F_Right] = 80;
			motor[B_Right] = 80;
		}

		if(Sector == 73 || Sector == 1)	//Stop
		{
			motor[F_Left] = 0;
			motor[B_Left] = 0;
			motor[F_Right] = 0;
			motor[B_Right] = 0;
			break;
		}
  }
}
